using BepuPhysics.CollisionDetection;
using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;

namespace BepuPhysics
{
    /// <summary>
    /// 增量更改一组实体的布局,以最大限度地减少与解算器和其他依赖连接跟随的系统相关联的缓存未命中。
    /// </summary>
    public partial class BodyLayoutOptimizer
    {
        Bodies bodies;
        BroadPhase broadPhase;
        Solver solver;

        float optimizationFraction;
        /// <summary>
        /// 获取或设置更新每个帧的所有正文的比例。
        /// </summary>
        public float OptimizationFraction
        {
            get
            {
                return optimizationFraction;
            }
            set
            {
                if (value > 1 || value < 0)
                    throw new ArgumentException("Optimization fraction must be a value from 0 to 1.");
                optimizationFraction = value;
            }
        }

        public BodyLayoutOptimizer(Bodies bodies, BroadPhase broadPhase, Solver solver, BufferPool pool, float optimizationFraction = 0.005f)
        {
            this.bodies = bodies;
            this.broadPhase = broadPhase;
            this.solver = solver;
            OptimizationFraction = optimizationFraction;

        }

        public static void SwapBodyLocation(Bodies bodies, Solver solver, int a, int b)
        {
            Debug.Assert(a != b, "Swapping a body with itself isn't meaningful. Whaddeyer doin?");
            // 枚举实体的当前约束集,将每个约束中的参照更改为新位置。
            // 请注意,必须更改对两个实体的引用-两个实体都已移动！
            // 此函数不会更新列表在图形中的实际位置,因此我们可以修改这两个位置,而不必担心索引无效。
            solver.UpdateForBodyMemorySwap(a, b);

            // 更新主体位置。
            bodies.ActiveSet.Swap(a, b, ref bodies.HandleToLocation);
            // TODO：如果主体布局优化器在所有其他阶段之前或之后发生,则不需要此交换。如果我们把它移到其他阶段之间,我们需要保持惯性
            // 与其他身体属性相一致。
            // Helpers.Swp(ref body.Inertias[a],ref body.Inertias[b]);
        }

        int nextBodyIndex = 0;

        struct IncrementalEnumerator : IForEach<int>
        {
            public Bodies bodies;
            public BroadPhase broadPhase;
            public Solver solver;
            public int Index;
            public int TargetIndexStart;
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void LoopBody(int connectedBodyIndex)
            {
                ++Index;
                if (Index > 32)
                    return;
                // 只有靠右的身体才能靠边。这有助于限制毫无意义的战斗。
                // 在这种情况下,岛内的物体将倾向于向最左侧身体的位置移动。
                // 如果没有它,在岛级收敛方面的任何进展都可能在下一次迭代中化为乌有。
                var newLocationIndex = TargetIndexStart + Index;
                if (connectedBodyIndex > newLocationIndex)
                {
                    // 请注意,我们会立即更新内存位置。这可能会影响下一次循环迭代。
                    // 但这很好;下一次迭代将从修改后的数据加载,所有内容都将保持一致。

                    // TODO：这个实现几乎肯定可以改进-
                    // 这个版本经过了所有的努力,潜入到类型批次中进行引用,然后再重新做一次来移动东西。
                    // 硬编码的交换操作可以同时完成这两个操作,从而节省了一些间接操作。
                    // 它不会那么快--每个单独的间接地址都已经被缓存了。
                    // 另外,在你做这类事情之前,要记住这个阶段有多短。
                    // 请注意,raph.EnumerateConnectedBody显式排除了我们正在枚举其约束的主体,
                    // 所以我们不必担心这个清单掉期会把地毯拉下水。
                    // (另外,！(X>x)表示许多x值。)
                    SwapBodyLocation(bodies, solver, connectedBodyIndex, newLocationIndex);
                }
            }
        }
        public void IncrementalOptimize()
        {
            // 所有这些都是寻找在给定物体右侧的任何物体。如果它找到了一个,它就会把它拉到相邻的位置。
            // 这在孤岛级别收敛-也就是说,在模拟孤岛的静电拓扑上运行此命令最终将导致
            // 这些岛在存储器中是相邻的,并且至少一些连接体彼此相邻。
            // 然而,在这些岛屿内,它可能会继续不必要地交换物体,因为身体会"争夺"所有权。
            // 一个身体不知道另一个身体在孩提时代已经认领了一个身体,所以这不能产生一个连贯的唯一的遍历顺序。
            // (事实上,即使使用单一的一维体体链,它通常也不会收敛。)

            // 与其他选项(如全岛遍历)相比,此优化例程需要的开销要少得多。我们只要求连接一个单独的身体,
            // 并且交换计数被限制为连接的实体的数量。

            // 如果无法执行任何优化,请不要费心进行优化。此条件在Worker执行过程中假定。
            if (bodies.ActiveSet.Count <= 2)
                return;
            int optimizationCount = (int)Math.Max(1, Math.Round(bodies.ActiveSet.Count * optimizationFraction));
            for (int i = 0; i < optimizationCount; ++i)
            {
                // 试图优化最后两具身体是没有意义的。不可能进行任何优化。
                if (nextBodyIndex >= bodies.ActiveSet.Count - 2)
                    nextBodyIndex = 0;

                var enumerator = new IncrementalEnumerator();
                enumerator.bodies = bodies;
                enumerator.broadPhase = broadPhase;
                enumerator.solver = solver;
                enumerator.TargetIndexStart = nextBodyIndex + 1;
                enumerator.Index = 0;
                bodies.EnumerateConnectedBodyIndices(nextBodyIndex, ref enumerator);

                ++nextBodyIndex;
            }

        }
    }
}
